#pragma once

#include <chrono>
#include <robot_chassis/CTurnOnRobot.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>

/**
 * @brief IMU节点，负责发布IMU数据
 * 
 * 从STM32获取加速度计和陀螺仪数据，发布到标准的 /chassis/imu 话题，
 * 并广播 base_link -> imu_link 的TF变换（imu_link 相对于 base_link 的位置和姿态）
 */
class CRobotImu : public rclcpp::Node
{
  public:
    explicit CRobotImu(const std::string& nodeName);
    ~CRobotImu();

  private:
    void getImuData();
    void publishTF();
    void callback();

    rclcpp::TimerBase::SharedPtr                        timer_;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_;
    std::unique_ptr<tf2_ros::TransformBroadcaster>      tfBroadcaster_;
    CTurnOnRobot&                                       turnonRobot_;
    sensor_msgs::msg::Imu                               sensorImuData_;
    SImuData                                            imuData_;

    // 参数
    std::string imuFrameId_{"imu_link"};
    std::string baseFrameId_{"base_link"};
    bool        publishTf_{true};
    
    // IMU 到 base_link 的变换参数
    double transformX_{0.0};      // X 偏移 (m)
    double transformY_{0.0};      // Y 偏移 (m)
    double transformZ_{0.0};      // Z 偏移 (m)
    double transformRoll_{0.0};   // Roll 角 (rad)
    double transformPitch_{0.0};  // Pitch 角 (rad)
    double transformYaw_{0.0};    // Yaw 角 (rad)
};
